package com.ecust.ml.grid.common;

import java.util.Iterator;

import com.fly.lib.util.JsonUtil;

/**
 * 网格
 * @author pleasebugme
 *
 */
public class Grid {
	
	/**
	 * 移动方向偏移概率[制定方向，左侧偏移，右侧偏移，相反方向]
	 */
	public static final float[] excursionProbability = new float[]{0.8f, 0.1f, 0.1f, 0.0f};

	/**
	 * 伽马值，越大则每个action的后续影响值越重要
	 */
	public static final float GAMA = 0.99f;
	
	public static final float INIT_REWARD = -0.02f;
	
	public static final float MAX_ERROR = 0.01f;
	
	/**
	 * 网格单元格
	 */
	private GridCell[][] gridCells;
	
	/**
	 * 构造函数
	 * @param connection 连通性矩阵
	 * @param rewards reward矩阵
	 */
	public Grid(int[][] connection, float[][] rewards) {
		int rowSize = connection.length;
		int columnSize = connection[0].length;
		
		// 连通性矩阵与reward矩阵维度必须一致
		if(rowSize != rewards.length ||
				columnSize != rewards[0].length) {
			System.out.println("参数错误：维度不同");
			throw new RuntimeException();
		}
		
		gridCells = new GridCell[rowSize][columnSize];
		for(int i=0; i<rowSize; i++) {
			for(int j=0; j<columnSize; j++) {
				GridCell cell = new GridCell(i * columnSize + j + 1, i, j, connection[i][j]==0, rewards[i][j]);
				gridCells[i][j] = cell;
			}
		}
	}
	
	/**
	 * 计算某单元格往某方向移动对应的状态转移结果
	 * @param index 网格单元索引值
	 * @param direction 方向
	 * @return 状态转移概率
	 */
	public TransitionProbability getTP(int index, Direction direction) {
		
		int[] position = this.getXYByIndex(index);
		int positionX = position[0];
		int positionY = position[1];
		
		TransitionProbability tp = new TransitionProbability();
		
		if(Direction.W == direction) {
			// 指定方向
			int newX = positionX - 1;
			int newY = positionY;
			if(this.isTargetAccessible(newX, newY)) {
				tp.addIndexProbability(this.getIndexByXY(newX, newY), excursionProbability[0]);
			} else {
				tp.addIndexProbability(this.getIndexByXY(newX+1, newY), excursionProbability[0]);
			}
			
			// 左侧偏移
			newX = positionX;
			newY = positionY + 1;
			if(this.isTargetAccessible(newX, newY)) {
				tp.addIndexProbability(this.getIndexByXY(newX, newY), excursionProbability[1]);
			} else {
				tp.addIndexProbability(this.getIndexByXY(newX, newY-1), excursionProbability[1]);
			}
			
			// 右侧偏移
			newX = positionX;
			newY = positionY - 1;
			if(this.isTargetAccessible(newX, newY)) {
				tp.addIndexProbability(this.getIndexByXY(newX, newY), excursionProbability[2]);
			} else {
				tp.addIndexProbability(this.getIndexByXY(newX, newY+1), excursionProbability[2]);
			}
			
			// 相反方向
			newX = positionX + 1;
			newY = positionY;
			if(this.isTargetAccessible(newX, newY)) {
				tp.addIndexProbability(this.getIndexByXY(newX, newY), excursionProbability[3]);
			} else {
				tp.addIndexProbability(this.getIndexByXY(newX-1, newY), excursionProbability[3]);
			}
		} else if(Direction.E == direction){
			// 指定方向
			int newX = positionX + 1;
			int newY = positionY;
			if(this.isTargetAccessible(newX, newY)) {
				tp.addIndexProbability(this.getIndexByXY(newX, newY), excursionProbability[0]);
			} else {
				tp.addIndexProbability(this.getIndexByXY(newX-1, newY), excursionProbability[0]);
			}
			
			// 左侧偏移
			newX = positionX;
			newY = positionY - 1;
			if(this.isTargetAccessible(newX, newY)) {
				tp.addIndexProbability(this.getIndexByXY(newX, newY), excursionProbability[1]);
			} else {
				tp.addIndexProbability(this.getIndexByXY(newX, newY+1), excursionProbability[1]);
			}
			
			// 右侧偏移
			newX = positionX;
			newY = positionY + 1;
			if(this.isTargetAccessible(newX, newY)) {
				tp.addIndexProbability(this.getIndexByXY(newX, newY), excursionProbability[2]);
			} else {
				tp.addIndexProbability(this.getIndexByXY(newX, newY-1), excursionProbability[2]);
			}
			
			// 相反方向
			newX = positionX - 1;
			newY = positionY;
			if(this.isTargetAccessible(newX, newY)) {
				tp.addIndexProbability(this.getIndexByXY(newX, newY), excursionProbability[3]);
			} else {
				tp.addIndexProbability(this.getIndexByXY(newX+1, newY), excursionProbability[3]);
			}
		} else if(Direction.N == direction){
			// 指定方向
			int newX = positionX;
			int newY = positionY - 1;
			if(this.isTargetAccessible(newX, newY)) {
				tp.addIndexProbability(this.getIndexByXY(newX, newY), excursionProbability[0]);
			} else {
				tp.addIndexProbability(this.getIndexByXY(newX, newY+1), excursionProbability[0]);
			}
			
			// 左侧偏移
			newX = positionX - 1;
			newY = positionY;
			if(this.isTargetAccessible(newX, newY)) {
				tp.addIndexProbability(this.getIndexByXY(newX, newY), excursionProbability[1]);
			} else {
				tp.addIndexProbability(this.getIndexByXY(newX+1, newY), excursionProbability[1]);
			}
			
			// 右侧偏移
			newX = positionX + 1;
			newY = positionY;
			if(this.isTargetAccessible(newX, newY)) {
				tp.addIndexProbability(this.getIndexByXY(newX, newY), excursionProbability[2]);
			} else {
				tp.addIndexProbability(this.getIndexByXY(newX-1, newY), excursionProbability[2]);
			}
			
			// 相反方向
			newX = positionX;
			newY = positionY + 1;
			if(this.isTargetAccessible(newX, newY)) {
				tp.addIndexProbability(this.getIndexByXY(newX, newY), excursionProbability[3]);
			} else {
				tp.addIndexProbability(this.getIndexByXY(newX, newY-1), excursionProbability[3]);
			}
		} else if(Direction.S == direction){
			// 指定方向
			int newX = positionX;
			int newY = positionY + 1;
			if(this.isTargetAccessible(newX, newY)) {
				tp.addIndexProbability(this.getIndexByXY(newX, newY), excursionProbability[0]);
			} else {
				tp.addIndexProbability(this.getIndexByXY(newX, newY-1), excursionProbability[0]);
			}
			
			// 左侧偏移
			newX = positionX + 1;
			newY = positionY;
			if(this.isTargetAccessible(newX, newY)) {
				tp.addIndexProbability(this.getIndexByXY(newX, newY), excursionProbability[1]);
			} else {
				tp.addIndexProbability(this.getIndexByXY(newX-1, newY), excursionProbability[1]);
			}
			
			// 右侧偏移
			newX = positionX - 1;
			newY = positionY;
			if(this.isTargetAccessible(newX, newY)) {
				tp.addIndexProbability(this.getIndexByXY(newX, newY), excursionProbability[2]);
			} else {
				tp.addIndexProbability(this.getIndexByXY(newX+1, newY), excursionProbability[2]);
			}
			
			// 相反方向
			newX = positionX;
			newY = positionY - 1;
			if(this.isTargetAccessible(newX, newY)) {
				tp.addIndexProbability(this.getIndexByXY(newX, newY), excursionProbability[3]);
			} else {
				tp.addIndexProbability(this.getIndexByXY(newX, newY+1), excursionProbability[3]);
			}
		} else {
			System.out.println("参数异常：未知的方向->" + direction);
		}
		
		return tp;
	}
	
	/**
	 * 由单元格x/y坐标计算其索引值
	 * @param index
	 * @return
	 */
	public int[] getXYByIndex(int index) {
		int rowSize = gridCells[0].length;
		int y = (index - 1) / rowSize;
		int x = (index - 1) % rowSize;
		return new int[]{x, y};
	}
	
	/**
	 * 由单元格索引值计算x/y坐标
	 * @param x
	 * @param y
	 * @return
	 */
	public int getIndexByXY(int x, int y) {
		return y * gridCells[0].length + x + 1;
	}
	
	/**
	 * 判断目标点是否可达（非边界或障碍物）
	 * @param x
	 * @param y
	 * @return
	 */
	public boolean isTargetAccessible(int x, int y) {
		if(x < 0 || x > gridCells[0].length - 1 ||
				y < 0 || y > gridCells.length - 1) {
			return false;
		} else if(gridCells[y][x].isObstacle()) {
			return false;
		} else {
			return true;
		}
	}
	
	/**
	 * 以可视化形式打印出网格数据
	 */
	public void printGrid() {
		
	}
	
	/**
	 * 计算值迭代
	 */
	public void valueIterations() {
		float[] value = new float[gridCells.length * gridCells[0].length];
		float[] newValue = new float[gridCells.length * gridCells[0].length];
		
		int iteratorCount = 0;
		while(true) {
			iteratorCount++;
			for(int i=0; i<value.length; i++) {
				// 对于障碍物不计算
				int x = this.getXYByIndex(i+1)[0];
				int y = this.getXYByIndex(i+1)[1];
				if(this.gridCells[y][x].isObstacle()) {
					continue;
				}
				
				// 所有可能的移动方向
				Direction[] directions = Direction.values();
				float maxSumQStar = 0;
				for(int j=0; j<directions.length; j++) {
					float sumQStar = 0;
					TransitionProbability tp = this.getTP(i+1, directions[j]);
					Iterator<Integer> indexes = tp.getGridCellIndexes().keySet().iterator();
					while(indexes.hasNext()) {
						int index = indexes.next();
						sumQStar += tp.getGridCellProbability().get(index) * value[index - 1];
					}
					
					if(sumQStar > maxSumQStar) {
						maxSumQStar = sumQStar;
					}
				}
				if(this.getReward(i+1) >= 1 || this.getReward(i+1) <= -1) {
					newValue[i] = this.getReward(i+1);
				} else {
					newValue[i] = this.getReward(i+1) + maxSumQStar;
				}
			}
			
			System.out.println(JsonUtil.toJsonString(newValue));
			// 检测是否已经收敛
			if(this.isConverge(value, newValue, MAX_ERROR)) {
				System.out.println("结果已经收敛，迭代次数：" + iteratorCount);
				// 打印
				for(int i=0; i<value.length; i++) {
					if(i%4 == 0) {
						System.out.println();
					}
					System.out.printf("%+.4f, ", value[i]);
					
				}
				break;
			}
			
			for(int i=0; i<value.length; i++) {
				value[i] = newValue[i];
			}
		}
	}
	
	/**
	 * 检测两次结果值是否已经满足收敛条件（每个元素差值小于maxError）
	 * @param oldValue 旧值
	 * @param newValue 新值
	 * @param maxError 最大差值
	 * @return
	 */
	public boolean isConverge(float[] oldValue, float[] newValue, float maxError) {
		System.out.println(JsonUtil.toJsonString(newValue));
		boolean converged = true;
		for(int i=0; i<oldValue.length; i++) {
			if(Math.abs(oldValue[i] - newValue[i]) > maxError) {
				converged = false;
				break;
			}
		}
		return converged;
	}
	
	/**
	 * 由单元格索引计算reward值
	 * @param index
	 * @return
	 */
	public float getReward(int index) {
		int[] position = this.getXYByIndex(index);
		return this.gridCells[position[1]][position[0]].getReward();
	}

	public GridCell[][] getGridCells() {
		return gridCells;
	}

	public void setGridCells(GridCell[][] gridCells) {
		this.gridCells = gridCells;
	}
	
	public static void main(String[] args) {
		int[][] connection = new int[][]{
				{1, 1, 1, 1},
				{1, 0, 1, 1},
				{1, 1, 1, 1}
		};
		
		float[][] reward = new float[][] {
				{INIT_REWARD, INIT_REWARD, INIT_REWARD, 		   1},
				{INIT_REWARD, 			0, INIT_REWARD, 		  -1},
				{INIT_REWARD, INIT_REWARD, INIT_REWARD, INIT_REWARD	}
		};	
		
		Grid grid = new Grid(connection, reward);
		//System.out.println( JsonUtil.toJsonString(grid.getTP(5, Direction.W)) );
		grid.valueIterations();
	}
	
}
